#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time    : 2018\9\7 0007 12:46
# @Author  : never mind
# @File    : circle_move.py

import numpy as np
import os
import copy
import utils
import constants as cons


def construct_trace_step_part3(pairs, target, free_point, neighbor):
    '''
    三个位置移动
    :param pairs:
    :param free_point: [num1, num2]
    :param neighbor: [[num1, num2], 'moved']
    :return:
    '''
    pairs.append([[target, free_point, neighbor], [neighbor, free_point]])
    return free_point

def construct_trace_step_part2(pairs, free_point, target, neighbor):
    '''
    两个位置移动
        target      point               point       point
        free_point  point       -->     free_point  target

    :param pairs:
    :param free_point: [num1, num2]
    :param neighbor1: [[num1, num2], 'moved']
    :param neighbor2:
    :return:
    '''
    pairs.append([[target, free_point], [free_point, target, neighbor]])
    free_point = target

    return free_point

def construct_trace_step5(free_point, target):
    neighbor_1, neighbor_2, neighbor_3, neighbor_4 = utils.get_circle_neighbors_setp5(free_point, target)
    pairs = []
    target_temp = copy.copy(target)

    # 回到neighbor4, 将neighbor4移到free_point
    construct_trace_step_part3(pairs, target, free_point, neighbor_4[0])
    construct_trace_step_part3(pairs, free_point, neighbor_4[0], neighbor_3[0])
    construct_trace_step_part3(pairs, neighbor_4[0], neighbor_3[0], neighbor_2[0])
    construct_trace_step_part3(pairs, neighbor_3[0], neighbor_2[0], neighbor_1[0])
    construct_trace_step_part3(pairs, neighbor_2[0], neighbor_1[0], list(target_temp))

    # 此次移动结束之后，变换 target free_point
    target = neighbor_1[0]
    free_point = list(target_temp)

    return pairs, free_point, target

def construct_trace_step4(free_point, target, trace):
    '''
    需要 将小车移动到枝干之后的(car_parking.move_out()) 新的状态下的 free_point, target
    :param free_point:
    :param target:
    :return:
    '''
    # target, target_next = get_point_from_trace(trace=trace)
    # neighbor_1: [[num1, num2], 'moved']
    neighbor_1, neighbor_2 = utils.get_circle_neighbors_setp4(free_point, target)
    pairs = []
    free_point_temp = tuple(free_point)
    # 将target沿着轨迹线往外移出一位
    free_point = construct_trace_step_part2(pairs, free_point, target, neighbor_2[0])

    free_point = construct_trace_step_part2(pairs, free_point, neighbor_2[0], neighbor_1[0])

    free_point = construct_trace_step_part2(pairs, free_point, neighbor_1[0], list(free_point_temp))

    pairs.append([list(free_point_temp), neighbor_1[0]])

    # 重新调整 target, free_point
    target = neighbor_1[0]
    free_point = list(free_point_temp)

    # free_point = construct_trace_step_part3(pairs, free_point, neighbor_2[0], neighbor_1[0])
    # free_point = construct_trace_step_part2(pairs, free_point, neighbor_1[0])
    # free_point = construct_trace_step_part(pairs, free_point, target)

    # pairs.append([free_point, target])
    # pairs.append([target, free_point])
    # free_point = target

    # pair.append([free_point, neighbor_2])
    # pair.append([neighbor_2, free_point])
    #
    # free_point = neighbor_2
    #
    # pair.append([free_point, target])
    # pair.append([target, free_point])
    #
    # free_point = target

    return pairs, free_point, target

def get_x_neighbor(source, target):
    '''
    在 x 方向上找到距离 target 较近的点
    :param source:
    :param target:
    :return:
    '''
    x_right = 1 if source[0] - target[0] > 0 else 0         # source 在 target 的右侧
    if x_right:
        return [source[0]-1, source[1]], x_right
    else:
        return [source[0]+1, source[1]], x_right

def get_y_neighbor(source, target):
    '''
    在 y 方向上找到距离 target 较近的点
    :param source:
    :param target:
    :return:
    '''
    x_top = 1 if source[1] - target[1] > 0 else 0
    if x_top:
        return [source[0], source[1]-1], x_top
    else:
        return [source[0], source[1]+1], x_top

def append_x_neighbors(pair, point, x_right=False):
    if x_right:
        pair = pair.append([point[0] - 1, point[1]])
    else:
        pair = pair.append([point[0] + 1, point[1]])

    return pair

def append_y_neighbors(pair, point, y_top=False):
    if y_top:
        pair = pair.append([point[0], point[1] - 1])
    else:
        pair = pair.append([point[0], point[1] + 1])

    return pair

def update_occ_free(new_occ, old_occ, new_free, old_free):
    old_occ.pop(0)
    old_occ.append(new_occ)
    old_free.pop(0)
    old_free.append(new_free)
    return old_occ, old_free

def get_all_position_in_trace(first_free, target):
    '''
    找到路径上所有的点，一共有四个方向， to_top, to_left, to_bottom, to_right 四个方向组合
    :param first_free:
    :param target:
    :return:
    '''
    x_neighbor, x_right = get_x_neighbor(first_free, target)
    y_neighbor, y_top = get_y_neighbor(first_free, target)

    # 找到路径上所有的点，一共有四个方向， to_top, to_left, to_bottom, to_right 四个方向组合
    x_num = np.abs(target[0] - first_free[0])
    y_num = np.abs(target[1] - first_free[1])
    to_top, to_left, to_bottom, to_right = False, False, False, False
    # to_top
    pairs = []
    temp_free = copy.copy(first_free)

    # paris 不包含 temp_free 位置， 构建的时候要更新 temp_free
    # pairs.append(temp_free)

    # free_point right-left
    for x in range(x_num):
        # append_x_neighbors(pair, temp_free, x_right=x_right)
        if x_right:
            temp_free = [temp_free[0] - 1, temp_free[1]]
        else:
            temp_free = [temp_free[0] + 1, temp_free[1]]
        pairs.append(temp_free)

    # temp_free = copy.copy(target)
    # target top-bottom
    for y in range(y_num):
        if y_top:
            temp_free = [temp_free[0], temp_free[1] - 1]
        else:
            temp_free = [temp_free[0], temp_free[1] + 1]
        pairs.append(temp_free)

    # temp_free = copy.copy(target)
    # target right-left
    for x in range(x_num):
        if not x_right:
            temp_free = [temp_free[0] - 1, temp_free[1]]
        else:
            temp_free = [temp_free[0] + 1, temp_free[1]]
        pairs.append(temp_free)

    # temp_free = copy.copy(first_free)
    # free_point top-bottom
    for y in range(y_num):
        # append_y_neighbors(pair, temp_free, y_top=y_top)
        if not y_top:
            temp_free = [temp_free[0], temp_free[1] - 1]
        else:
            temp_free = [temp_free[0], temp_free[1] + 1]
        pairs.append(temp_free)
    return pairs

def move_to_free(target, free):
    '''
    get all points from target to free
    :param target:
    :param free:
    :return:
    '''
    traces = []
    x_min = target[0] if target[0] < free[0] else free[0]
    x_max = target[0] if target[0] > free[0] else free[0]
    y_min = target[1] if target[1] < free[1] else free[1]
    y_max = target[1] if target[1] > free[1] else free[1]

    reverse = True if target[0] > free[0] else False

    for i in range(x_min, x_max+1):
        for j in range(y_min, y_max+1):
            traces.append([i, j])
    if reverse:
        traces.reverse()

    return traces

def construct_trace_by_points(trace_points):
    '''
    trace_points: [[free], [pos], [pos], [target]]
    :param trace_points:
    :return:
    '''
    traces = []
    traces.append([[], trace_points[:-1]])         # 首先加入 from target to first obstacle 的路径
    trace_points = copy.copy(trace_points)
    trace_points.reverse()
    for index in range(len(trace_points) - 2):
        trace = []
        trace.append([trace_points[index + 1], trace_points[index]])
        trace.append([trace_points[index], trace_points[index+1], trace_points[index+2]])
        traces.append(trace)
        pass
    traces.append([[trace_points[-1], trace_points[-2]], [trace_points[-2], trace_points[-1]]])
    # traces.append([[], [trace_points[index], trace_points[index+1]]])
    return traces
# construct_trace_by_points([[5, 5], [4, 5], [3, 5], [2, 5]])

def construct_second_traces(pairs, hold_first_free, another_free, traces, method=cons.one_sub_root):
    if method == cons.one_sub_root:       # 仅仅存在一个空位置
        pass_point = False
        target = []
        temp_free = copy.copy(pairs[-1])
        target_now = pairs[-2]
        # 第二个阶段，每次循环保证空位置是在target的下个位置
        while True:
            for index in range(len(pairs) - 1):
                trace = []
                # first_obstacle = pairs[0]
                # temp_free = pairs[-1]
                # target = pairs[-2 * (index + 1)]

                if not pass_point:
                    # stage one: move car
                    trace.append([pairs[index], temp_free])
                    # stage two: move flat car
                    trace.append([temp_free, pairs[index], pairs[index + 1]])

                    traces.append(trace)
                    temp_free = pairs[index]
                    pass_point = False
                else:
                    trace.append([])
                    trace.append([pairs[0], pairs[1]])

                    traces.append(trace)
                    temp_free = pairs[0]
                    pass_point = False
                    keep_free = pairs[-1]  # 更新处于free状态
                # if pairs[1] in all_free:        # 下一个障碍物位置没有被占用, 跳过移动该位置
                #     pass_point = True
                # else:
                #     pass
                # 每次回合结束，更新 all_free 以及 obstacle
                # tmp_obstacle, all_free = update_occ_free(pairs[index], tmp_obstacle, pairs[index + 1], all_free)

                # temp_free = pairs[index]

                # 重新构造 pairs 形成一个循环，此时以下一个障碍物为第一个元素，target 为倒数第二个元素， first_free 为最后一个元素
                # pairs.insert(0, pairs[-1])
                # pairs.pop(-1)
                # target_now = pairs[-2 * (index + 1)]

                # 更新pairs

                # 跳出原则

            temp_pairs = pairs[-1]
            pairs.insert(0, temp_pairs)
            pairs.pop(-1)

            target = pairs[-2]
            if target == hold_first_free:
                break

            # for index in range(len(pairs)):
            #     pairs.append(pairs[0])
            #     pairs.remove(pairs[0])

            # if target_now == first_free_temp:
            #     break

            # if len(obstacle) != 0 and pairs[-1] in obstacle:
            #     obstacle.remove(pairs[-1])
            #
            # if pairs[-2] == first_free_temp:
            #     break
        # 处理最后一个路径
        last_trace = traces[-1]
        traces.pop(-1)
        traces.append([last_trace[0], []])
        print('total steps: %s.' % len(traces))
        return traces
    elif method == cons.two_sub_root:         # 存在两个空位置
        pass_point = False
        # target = []
        temp_free = copy.copy(pairs[-1])
        # target_now = pairs[-2]
        # move_add_free = False
        # end_true = False

        # 第二个阶段，每次循环保证空位置是在target的下个位置
        while True:
            # all_free = [pairs[1], pairs[-1]]
            # obstracal_tmp = pairs[0]
            # flat_tmp = pairs[0]
            free_1 = pairs[-1]
            # free_2 = pairs[1]
            # target_tmp = pairs[-2]
            # target = pairs[-2]

            for index in range(len(pairs) - 1):
                trace = []
                obstracal_tmp = pairs[index]
                flat_tmp = pairs[index]
                # free_1 = pairs[index - 1]
                free_2 = pairs[index + 1]
                target_tmp = pairs[index - 2]
                free_list = [free_1, another_free]
                if pairs[index] in free_list:        # 下一个障碍物位置没有被占用, 跳过移动该位置
                    pass_point = True
                else:
                    pass

                if not pass_point:
                    # stage one: move car
                    move_add_free = False
                    if move_add_free:       # 一次移动两个free的位置
                        trace.append([pairs[index], temp_free, ])
                        trace.append([temp_free, pairs[index], pairs[index + 1]])
                        pass
                    else:
                        trace.append([pairs[index], temp_free])
                        # stage two: move flat car  pairs[index]: 指向上次的free_1, 这次的
                        trace.append([temp_free, pairs[index], pairs[index + 1]])

                    traces.append(trace)
                    temp_free = pairs[index]    # 保留上次
                    free_1 = pairs[index]       # 每次完成移动之后，原先是obstacle的成为新的free
                    pass_point = False
                else:
                    # 如果该位置是空位置，前移两个使得中间位置没有空位
                    trace.append([])
                    trace.append([pairs[index + 1]])

                    traces.append(trace)
                    temp_free = pairs[index]
                    another_free = pairs[index - 1]     # another_free = free_1
                    free_1 = pairs[index]
                    print('another free %s' % another_free)
                    pass_point = False
                    move_add_free = True


            temp_pairs = pairs[-1]
            pairs.insert(0, temp_pairs)
            pairs.pop(-1)

            # 最后一个 trace 中的 第一个阶段的 最后一个元素
            target = traces[-1][0][-1]
            if target == hold_first_free:
                obstracal_tmp = pairs[0]
                flat_tmp = pairs[0]
                free_1 = pairs[-1]
                free_2 = pairs[1]
                target_tmp = pairs[-2]
                target = pairs[-2]
                break

        # 处理最后一个路径
        last_trace = copy.copy(traces[-1])
        traces.pop(-1)
        # 增加连接处
        traces.append([last_trace[0], []])


        # 处理剩余部分，使得空位在主干道上
        sub_root_node = list()
        for item in pairs:
            node_info = utils.check_position_info(item[0], item[1])
            if node_info == cons.node_sub_root and item[0] == hold_first_free[0]:
                # return item
                sub_root_node = item
                break
        # 增加连接处
        traces.append([[], [sub_root_node]])
        # m_target = [5, 5]
        # m_free = [3, 5]
        m_target, m_free = sub_root_node, another_free
        move_to_free_points = move_to_free(m_target, m_free)
        move_to_free_trace = construct_trace_by_points(move_to_free_points)
        traces.extend(move_to_free_trace)

        # 增加连接处
        traces.append([[], [target]])
        # traces.append([[], target])
        # last_trace[0].reverse()
        # traces.append([last_trace[0], []])
        print('total steps: %s.' % len(traces))
        return traces
        pass

def choose_method(first_free=None, pairs=None):
    '''
    判断是那种情况：只有一个空位置，或者有两个空位置
    1. if first free position is a main sub root node, choose 'one_sub_root'
    2. if there are two sub root node in pairs, choose 'two_sub_root', one for 'one_sub_root'
    :return:
    '''
    is_main_sub_root = utils.is_main_sub_root_node(first_free)
    if is_main_sub_root:
        return cons.two_sub_root
    else:
        return cons.one_sub_root
    # one way --- failure for sub_root_node
    node_info = utils.check_position_info(first_free[0], first_free[1])
    if node_info == cons.node_sub_root:
        return cons.two_sub_root
    else:
        return cons.one_sub_root
    pass
    # another way   ---     failure
    num_root = 0
    free_list = list()
    for item in pairs:
        node_info = utils.check_position_info(item[0], item[1])
        if node_info == cons.node_sub_root:
            num_root += 1
            free_list.append(item)
    if num_root == 2:
        return cons.two_sub_root, free_list
    else:
        return cons.one_sub_root, free_list

def get_main_sub_root_free(first_free, pairs):
    '''
    得到 first_free 的上下两个点，如果在 pairs中，说明其实 free point
    :param first_free:
    :param pairs:
    :return:
    '''
    free_list = list()
    free_list.append(first_free)
    another_free = list()
    for item in [[first_free[0], first_free[1]+1], [first_free[0], first_free[1]-1]]:
        if item in pairs:
            free_list.append(item)
            another_free = item
        else:
            print('there is no free main sub root. ')
    return free_list, another_free
    pass

def get_all_free_from_pairs(pairs):
    pass

def construct_trace_one_step(target, first_free, all_obstacle, out_pair, occupied):
    '''
    根据 target 和第一个 空位置 first_free 构建移动的路径
    :param target:
    :param first_free:
    :return:
    '''
    # target = [1, 0]
    # first_free = [4, 1]
    # neighbors = utils.get_circle_neighbors_one_step(target, first_free)             # 获取循环中的所有的点
    traces = list()         # 总的路径

    target_temp = copy.copy(target)
    hold_first_free = copy.copy(first_free) # 预留一个

    all_free = copy.copy(out_pair)

    obstacle = copy.copy(all_obstacle)
    tmp_obstacle = copy.copy(all_obstacle) # 移出路径上的所有障碍
    obstacle.remove(target)         # 首先去除目标点

    # 找到路径上所有的点
    pairs = get_all_position_in_trace(first_free, target)
    free_list, another_free = get_main_sub_root_free(hold_first_free, pairs)
    # for item in pairs:
    #     all_free.remove(item)

    # 构建循环路径
    # 从第一个障碍物开始，先把障碍物移到 free_point,
    # 然后移到第二个障碍物处，然后将第二个障碍物移到新的 free_point
    flat_car = ''
    temp_free = copy.copy(first_free)
    # 第一个阶段是将空位置移动到target的下个位置
    for index in range(len(pairs) - 1):
        trace = []
        # stage one
        trace.append([pairs[index], temp_free])
        # stage two
        trace.append([temp_free, pairs[index], pairs[index + 1]])
        traces.append(trace)

        if pairs[index] == target:
            flat_car = pairs[index + 1]
            first_free = pairs[index]
            target = temp_free
            break

        temp_free = pairs[index]
        pass

    # 重新构造 pairs 形成一个循环，此时以下一个障碍物为第一个元素，target 为倒数第二个元素， first_free 为最后一个元素
    for index in range(len(pairs)):
        pairs.append(pairs[0])
        pairs.remove(pairs[0])
        if pairs[0] == flat_car:
            break

    method = choose_method(first_free=hold_first_free, pairs=pairs)
    traces = construct_second_traces(pairs, hold_first_free, another_free, traces, method=method)
    if method == cons.one_sub_root:
        traces.append([all_free, []])
        return pairs, traces
    elif method == cons.two_sub_root:
        traces.append([all_free, []])
        return pairs, traces
        pass
    last_trace = traces[-1]
    last_second_trace = traces[-2]
    traces.pop(-1)

    if last_second_trace[0][1] in cons.pos_out:
        traces.pop(-1)
        traces.append([last_second_trace[0], []])
        return pairs, traces
    # 剩余最后一步，需要将目标移出，并把主要的子根路线空出来
    traces.append([last_trace[0], []])
    traces.append([out_pair, []])

    tmp_out_pair = copy.copy(out_pair)
    tmp_out_pair.reverse()
    tmp_out_pair.append(last_second_trace[0][-1])
    traces.append([tmp_out_pair, last_second_trace[-1]])

    # reset car
    traces.append([[], last_trace[0]])
    traces.append([[], out_pair])
    # 需要更新target, first_free 位置
    # first_free = pairs[-2]
    # target = pairs[-3]
    # flat_car = pairs[-1]

    return pairs, traces

def construct_trace(trace, occ):
    '''
        # pair_stage_1 = [[3, 1], [4, 1]]
        # pair_stage_2 = [[4, 1], [3, 1], [2, 1]]
        # pair = [pair_stage_1, pair_stage_2]
        # pairs = [pair, pair, pair]
        # [[[3, 1], [4, 1]], [[4, 1], [3, 1], [2, 1]]]
        # [[[2, 1], [3, 1]], [[3, 1], [2, 1], [1, 1]]]
        # [[[1, 1], [2, 1]], [[2, 1], [1, 1], [1, 2]]]
    :param trace:
    :param occ:
    :return:
    '''
    target, target_next, all_obstacle, first_free, first_obstacle, out_pair = get_point_from_trace(trace=trace, occ=occ)
    # all_obstacle, first_free = get_all_obstacle(trace, occ)

    pairs = []
    for i, item in enumerate(all_obstacle):
        pair = []
        if item == target:
            break
        pair_stage_1, pair_stage_2 = [], []
        pair_stage_1.append(all_obstacle[i])
        pair_stage_1.append(first_free)        # 移车

        pair_stage_2.append(first_free)
        pair_stage_2.append(all_obstacle[i])
        pair_stage_2.append(all_obstacle[i+1])
        # pair_item.append(all_obstacle[i+2])

        first_free = copy.copy(all_obstacle[i])

        pair.append(pair_stage_1)
        pair.append(pair_stage_2)

        pairs.append(pair)

    # 移出小车
    # pairs.append([target, target_next])
    return pairs

def update_free_point(old_point, new_point):
    old_point[0] = new_point[0]
    old_point[1] = new_point[1]
    return old_point

def get_first_obstacle(trace):
    '''
    根据轨迹找到第一个障碍点
    :param trace:
    :return:
    '''
    pass

def get_point_from_trace(trace, occ):
    '''
    沿着轨迹线，找到 target、target的下一个位置
    :param trace: [[pos], [pos], [pos]]
    :return: 目标位置，目标的下一个位置，所有的障碍点，第一个空闲位置
    '''
    # trace_0 = trace[0]
    # target = trace_0[-1]
    # target_next = trace_0[-2]

    obstacle, first_free = [], []
    out_pair = []
    first = True
    target = trace[-1]
    target_next = trace[-2]
    if len(trace) == 2 or len(trace) == 1:
        trace = trace[0]
        target = trace[-1]
        target_next = trace[-2]

    for trace_item in trace:
        if first:
            first_obstacle = trace_item
        if trace_item in occ:
            first = False
            obstacle.append(trace_item)
        if first:
            first_free = copy.copy(trace_item)
        if trace_item not in occ and first:
            out_pair.append(trace_item)

    out_pair.reverse()

    return target, target_next, obstacle, first_free, first_obstacle, out_pair

